#include "preparestate.h"
#include "fsm.h"
#include "maincontroller.h"
#include "masters/vehiclecontroller.h"
using namespace std;
using namespace vehicle;
PrepareState::PrepareState()
{

}


/**
 * @brief ReadyState::Update
 * @param fsm
 *
 */

void PrepareState::Update(FSM *fsm)
{
    //cout<<"ready state"<<endl;
    char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();

    ushort sn=0;
    int ret=dhdGetSerialNumber(&sn,master_device);
    if(ret!=0)
    {
        fsm->vehicle_controller_->SetMasterDeviceIndex(-1);
        master_device=-1;
    }


    switch (fsm->vehicle_controller_->GetRunningStatus())
    {
    case main_controller::kRCStop:
        fsm->SetState(BaseState::kSleepState);
        break;
    case main_controller::kRCWorking:
        if(fsm->vehicle_controller_->GetConnectionStatus()==main_controller::kConnectionOkay && master_device>=0)
            fsm->SetState(BaseState::kResetState);
        break;
    default:
        break;
    }
}

void PrepareState::OnEntry(FSM *fsm)
{
    cout<<"entry prepare state"<<endl;
    fsm->vehicle_controller_->StartRobotRC();
    char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();
    if(master_device<0)
        return;

    drdOpenID(master_device);
    drdStart(master_device);
    dhdSetForceAndGripperForce(0,0,0,0,master_device);

//    if(fsm->vehicle_controller_->SetMasterForce(Eigen::Vector3d::Zero()))
//        cout<<"failed to clear feedback force"<<endl;
}

void PrepareState::OnExit(FSM *fsm)
{
    cout<<"exit prepare state"<<endl;

}



